/***************************************************************************
 创建者: 华磊
 开始时间: 2018.10.25
 copyright: (C) 华友高新
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) 李文友 2017.5.8 类的具体实现代码编写

 ***************************************************************************/

#ifdef D_USE_UI
#include <QApplication>
#else
#include <QCoreApplication>
#endif
#include <ros/ros.h>
#include <std_msgs/UInt32.h>
#include <QDebug>

//分隔符有分号";" "&&" "||"。
//1、分号分隔：命令按顺序从左到右依次执行，如果某个命令执行出错了，直接跳到下一个命令继续执行！（这个比较危险，慎用）
//2、"&&"：这个和编程语言中的功能很相似，短路效果，如果某个命令执行出错了，后面的命令全部被忽略！
//3、"||"：忽略第一个有效命令后的所有命令。如果第一个命令就成功了，那后面的都被忽略；如果第一个命令失败，则继续执行第二个，如果第二个成功了，那后面的命令被忽略；以此类推..

void callback(const std_msgs::UInt32& msg)
{
    qDebug()<<"process command!"<<msg.data;
    switch(msg.data)
    {
    case 1://gmapping
    {
        std::string mapping_algorithm;
        int returnResult;

        mapping_algorithm="roslaunch tcr100 gmapping_with_one_lidar.launch &";

        const char* mapping_algorithm_sh=mapping_algorithm.c_str();
        int mapping_algorithm_status=system(mapping_algorithm_sh);
        if(mapping_algorithm_status==-1)
        {
            printf("gmapping mapping_algorithm system error... \n");
            returnResult=0;
        }
        else
        {

            if(WIFEXITED(mapping_algorithm_status))
            {
                if(0==WEXITSTATUS(mapping_algorithm_status))
                {
                    printf("gmapping mapping_algorithm run successfully... \n");
                    returnResult=1;
                }
                else
                {
                    printf("gmapping mapping_algorithm run failed %d \n",WEXITSTATUS(mapping_algorithm_status));
                    returnResult=0;
                }
            }
            else
            {
                printf("gmapping mapping_algorithm exit code %d \n",WEXITSTATUS(mapping_algorithm_status));
                returnResult=0;
            }
        }

        break;

    }
    case 2://cartographer
    {
        std::string mapping_algorithm;
        int returnResult;
        //source /home/tcr/catkin_ws/install_isolated/setup.sh
        mapping_algorithm="roslaunch cartographer_ros mapping_2d.launch &";

        const char* mapping_algorithm_sh=mapping_algorithm.c_str();
        int mapping_algorithm_status=system(mapping_algorithm_sh);
        if(mapping_algorithm_status==-1)
        {
            printf("cartographer mapping_algorithm system error... \n");
            returnResult=0;
        }
        else
        {

            if(WIFEXITED(mapping_algorithm_status))
            {
                if(0==WEXITSTATUS(mapping_algorithm_status))
                {
                    printf("cartographer mapping_algorithm run successfully... \n");
                    returnResult=1;
                }
                else
                {
                    printf("cartographer mapping_algorithm run failed %d \n",WEXITSTATUS(mapping_algorithm_status));
                    returnResult=0;
                }
            }
            else
            {
                printf("cartographer mapping_algorithm exit code %d \n",WEXITSTATUS(mapping_algorithm_status));
                returnResult=0;
            }
        }

        break;

    }
    }

}

#define D_VERSION "V1.1-20191018"

int main(int argc, char *argv[])
{
#ifdef D_USE_UI
    QApplication a(argc, argv);
#else
    QCoreApplication a(argc, argv);
#endif

    qDebug()<<"Map Creat Server!"<<D_VERSION;
    //设置优先级
    struct sched_param param;
    param.__sched_priority = 19;
    sched_setscheduler(0,SCHED_OTHER,&param);


    ros::init(argc, argv, "map_watting");
    ros::NodeHandle nh_;
    ros::Rate ros_rate(1);

    ros::Subscriber *command_sub_;
    command_sub_=new ros::Subscriber ;
    *command_sub_ = nh_.subscribe("/create_map", 10, &callback);
    qDebug()<<"wait for command!";

    while(nh_.ok())
    {
        ros::spinOnce();               // check for incoming messages
        ros_rate.sleep();


    }

//    a.exec();

    qDebug()<<" MapCreat Main thread exit, 程序退出 0！";
    return 0;
}


